program program_ALS
!use madx_ptc_module
!use pointer_lattice
USE madx_ptc_module
USE pointer_lattice
implicit none

    interface
       subroutine build_ALS(ALS,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: ALS
         logical(lp) mis
       end subroutine build_ALS
    end interface

type(layout), pointer:: ALS
type(damap) id,m,a0
real(dp) closed_orbit(6),prec,a,b,c,d,g,e,del,sq,ep(2),psi,phi(2),x1(6),x2(6)
type(real_8) ray(6)
type(internal_state) state
type(normalform) normal
integer :: no1=0,np1=0 
type(taylor) average_floquet,average_xp,radii(2),invariant(2)
integer  sta,i,mf,j
logical(lp) mis,tot
type(vecresonance) diagonal_field
type(taylor), allocatable :: tunes(:),damping(:)
type(pbfield) hrot
type(pbresonance) hr
logical threeqx,flat_print
character*48 command_gino
prec=1.d-10 ! for printing
use_info=.TRUE. 
call ptc_ini_no_append
    call append_empty_layout(m_u)

ALS=>m_u%start


Write(6,*) " small misalignments and rotations in lattice ? input t or f "
!read(5,*) mis
mis=.false.
Write(6,*) 
Write(6,*) " 3Q_x -> t "
Write(6,*) " 4Q_x -> f "
Write(6,*) 
read(5,*)  threeqx
flat_print=.true.

call build_ALS(ALS,mis) 


 call kanalnummer(mf,file="fit_tune.txt")
  write(mf,*) "select layout"
  write(mf,*) 1
  write(mf,*) " set families"
  write(mf,*) " 2" 
  write(mf,*) "1 qf1"
  write(mf,*) "2 1"
  write(mf,*) "1 qd1"
  write(mf,*) "2 2 "
  write(mf,*) "FIT TUNE"
  write(mf,*) "1.d-6 "
  if(threeqx) then
   write(mf,*) "0.335 .271234"
  else
   write(mf,*) "0.255 .271234"
  endif
  if(flat_print) then
   write(mf,*) "print flat file"
   if(threeqx) then
     write(mf,*) "als_third.txt"
    else
     write(mf,*) "als_fourth.txt"
    endif
   endif
  write(mf,*) " return "
 close(mf)

call read_ptc_command77("fit_tune.txt")



 state=only_4d0

 if(threeqx) then
  call kanalnummer(mf,file="results_3Qx.txt")
 else
  call kanalnummer(mf,file="results_4Qx.txt")
 endif



closed_orbit=0.d0 ! initial guess for closed orbit

call FIND_ORBIT(ALS,closed_orbit,1,state,1.d-5)

write(6,'(a16,6(1x,g12.5))') " closed orbit = ",closed_orbit(1:6)
write(mf,'(a16,6(1x,g12.5))') " closed orbit = ",closed_orbit(1:6)


  no1=4

 
np1=0    !!!  No system  parameters allowed in the stochastic map

call init(state,no1,np1) ! PTC and FPP are properly initialized

call alloc(id,m,a0)  
call alloc(ray)
call alloc(normal)
call alloc(diagonal_field)
call alloc(hrot)
call alloc(hr)
 



id=1;   ray=closed_orbit+id;   ! ray= closed orbit + identity map

CALL TRACK(ALS,RAY,1,STATE) ! One turn map is computed via the ray

                 ! 

!!! The ray contains a truncated power series algebra
!!! The map we are interested to compute is around the closed orbit: 
!!!  it is really part of a diffential algebra
m=ray   !   The ray is "officially" turned into a "damapspin"  (DA in Berz's honour)
!

if(threeqx) then

!
j=1
do i=3,c_%no,3
 normal%nres=j
 normal%m(1,j)=3*j;
 j=j+1
enddo
!
!
normal = m  !  The map is normalised into a single resonance  
m=normal%a_t**(-1)*m*normal%a_t
!
a0=normal%a_t

hrot=pi/3.d0*((1.d0.mono.'2')+(1.d0.mono.'02'))
m=texp(hrot,m)

normal%nres=0
normal=m

hrot%h=normal%normal%pb%h*normal%a_t**(-1)
hr=hrot
call clean_pbresonance(hr,hr,prec)
call print(hr,6)
g=hr%cos%h.sub.'1100'
a=hr%cos%h.sub.'3000'
c=hr%cos%h.sub.'2200'
d=hr%cos%h.sub.'1111'
e=hr%cos%h.sub.'0022'
b=hr%sin%h.sub.'3000'

sq=sqrt(a**2+b**2)
del=9*sq**2-8*c*g

ep(1)=(sqrt(del)-3*sq)/4/c
ep(2)=(sqrt(del)+3*sq)/4/c
 
psi=atan2(a,b)

phi(1)=(pi/2-psi)/3
phi(2)=(3*pi/2-psi)/3


x1=0.d0; x2=0.d0

x1(1)=ep(1)*cos(phi(1))
x1(2)=-ep(1)*sin(phi(1))
x2(1)=ep(2)*cos(phi(2))
x2(2)=-ep(2)*sin(phi(2))

id=0
id=x1
id=a0.o.id
x1=id
id=0
id=x2
id=a0.o.id
x2=id

write(mf,*)   
write(mf,*)  "  Results of 3Q_x resonance relative to central fixed point " 
write(mf,*)  

write(mf,'(a43,4(1x,g20.13))')  " unstable fix point (perturbation theory) ",x1(1:4)


x1=x1+closed_orbit
call FIND_ORBIT(ALS,x1,1,state,1.d-5,turns=3)
x1=x1-closed_orbit

write(mf,'(a43,4(1x,g20.13))')   " unstable fix point ( Exact calculation)  ",x1(1:4)

write(mf,'(a43,4(1x,g20.13))')   "   stable fix point (perturbation theory) ",x2(1:4)

x2=x2+closed_orbit
call FIND_ORBIT(ALS,x2,1,state,1.d-5,turns=3)
x2=x2-closed_orbit
write(mf,'(a43,4(1x,g20.13))')   "   stable fix point ( Exact calculation)  ",x2(1:4)


else

j=1
do i=4,c_%no,4
 normal%nres=j
 normal%m(1,j)=4*j;
 j=j+1
enddo
!
!
normal = m  !  The map is normalised into a single resonance
m=normal%a_t**(-1)*m*normal%a_t

a0=normal%a_t

hrot=pi/4.d0*((1.d0.mono.'2')+(1.d0.mono.'02'))
m=texp(hrot,m)

normal%nres=0
normal=m

hrot%h=normal%normal%pb%h*normal%a_t**(-1)
hr=hrot

call clean_pbresonance(hr,hr,prec)
call print(hr,6)
g=hr%cos%h.sub.'1100'
a=hr%cos%h.sub.'4000'
c=hr%cos%h.sub.'2200'
d=hr%cos%h.sub.'1111'
e=hr%cos%h.sub.'0022'
b=hr%sin%h.sub.'4000'

sq=sqrt(a**2+b**2)

ep(1)= sqrt(-g/(2*c+ 4*sq))
ep(2)=sqrt(-g/(2*c- 4*sq))




psi=atan2(a,b)

phi(1)=(pi/2-psi)/4
phi(2)=(3*pi/2-psi)/4


x1=0.d0; x2=0.d0

x1(1)=ep(1)*cos(phi(1))
x1(2)=-ep(1)*sin(phi(1))
x2(1)=ep(2)*cos(phi(2))
x2(2)=-ep(2)*sin(phi(2))

id=0
id=x1
id=a0.o.id
x1=id
id=0
id=x2
id=a0.o.id
x2=id

write(mf,*)   
write(mf,*)  "  Results of 4Q_x resonance relative to central fixed point " 
write(mf,*)  

write(mf,'(a43,4(1x,g20.13))')  " unstable fix point (perturbation theory) ",x1(1:4)


x1=x1+closed_orbit
call FIND_ORBIT(ALS,x1,1,state,1.d-5,turns=4)
x1=x1-closed_orbit

write(mf,'(a43,4(1x,g20.13))')   " unstable fix point ( Exact calculation)  ",x1(1:4)

write(mf,'(a43,4(1x,g20.13))')   "   stable fix point (perturbation theory) ",x2(1:4)

 
x2=x2+closed_orbit
call FIND_ORBIT(ALS,x2,1,state,1.d-8,turns=4)
x2=x2-closed_orbit
write(mf,'(a43,4(1x,g20.13))')   "   stable fix point ( Exact calculation)  ",x2(1:4)
 

endif

write(mf,*)

x1=x1+closed_orbit
x2=x2+closed_orbit

call track(als,x1,1,state)
call track(als,x1,1,state)
call track(als,x1,1,state) 
if(.not.threeqx)call track(als,x1,1,state)
write(mf,'(a57,4(1x,g20.13))')   "  Tracked unstable fix point ( Exact calculation)  ",x1(1:4)-closed_orbit(1:4)
call track(als,x2,1,state)
call track(als,x2,1,state)
call track(als,x2,1,state) 
if(.not.threeqx)call track(als,x2,1,state)
write(mf,'(a57,4(1x,g20.13))')   "  Tracked stable fix point ( Exact calculation)   ",x2(1:4)-closed_orbit(1:4)


write(6,*) "   "
write(6,*) " hit return to terminate program "
write(6,*) "   "
pause 

close(mf)
call set_lattice_pointers
999 command_gino="OPENGINO"    
  call context(command_gino)   ! context makes them capital
  call call_gino(command_gino)  
 111 command_gino="MINI" 
  call context(command_gino)   ! context makes them capital
  call call_gino(command_gino)  
 



!!!!!!!!! vaguelynecessary baloney
command_gino="CLOSEGINO"
  call call_gino(command_gino)  
end program program_ALS


!=================================================================


subroutine  build_ALS(ALS,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: ALS

real(dp) :: alpha,lbend, cut, ksd, ksf 
type(fibre)  L1,L2,L3,L4,L5,L6,L7,L8,L9,L10
type(fibre)  L11,L12,L13,L14,L15,L16,L17,L18,L19,L20
type(fibre)  L21,L22,L23,L24,L25,L26,L27,L27A,L27B,L27C,L27D,DS
 type(fibre)  QF1,QF2,QD1,QD2,QFA1,QFA2,sf,sd,cav,bend,vc5,bend1
type(layout) :: sfline,sdline,sup1,supb
logical(lp) mis
!-----------------------------------

call make_states(.true.)
exact_model = .false.
!default = default + nocavity  
call update_states
madlength = .false.


call set_mad(energy = 1.5d0, method = 6, step = 3)

madkind2 = drift_kick_drift


  L1  = drift("L1 ",  2.832695d0)
  L2  = drift("L2 ",  0.45698d0)
  L3  = drift("L3 ",  0.08902d0)
  L4  = drift("L4 ",  0.2155d0)
  L5  = drift("L5 ",  0.219d0)
  L6  = drift("L6 ",  0.107078d0)
  L7  = drift("L7 ",  0.105716d0)
  L8  = drift("L8 ",  0.135904d0)
  L9  = drift("L9 ",  0.2156993d0)
  L10 = drift("L10",  0.089084d0)
   L11= drift("L11",  0.235416d0)
   L12= drift("L12",  0.1245d0)
   L13= drift("L13",  0.511844d0)
   L14= drift("L14",  0.1788541d0)
   L15= drift("L15",  0.1788483d0)
   L16= drift("L16",  0.511849d0)
   L17= drift("L17",  0.1245d0)
   L18= drift("L18",  0.235405d0)
   L19= drift("L19",  0.089095d0)
   L20= drift("L20",  0.2157007d0)
   L21= drift("L21",  0.177716d0)
   L22= drift("L22",  0.170981d0)
   L23= drift("L23",  0.218997d0)
 L24 = drift ("L24",  0.215503d0)
 L25 = drift ("L25",  0.0890187d0)
 L26 = drift ("L26",  0.45698d0)
 L27 = drift ("L27",  2.832696d0)
 L27a  = drift (" L27a",  0.8596d0)
 L27b  = drift (" L27b",  0.1524d0)
 L27c  = drift (" L27c",  0.04445d0)
 L27d  = drift (" L27d",  1.776246d0)
 ds  = drift (" DS  ", 0.1015d0)

  QF1 = QUADRUPOLE(" QF1 ",0.344D0, K1= 2.2474D0+6.447435260914397D-03)
  QF2 = QUADRUPOLE(" QF2 ",0.344D0, K1= 2.2474D0)
  QD1 = QUADRUPOLE(" QD1 ",0.187D0, K1= -2.3368D0-2.593018157427161D-02); 
  QD2 = QUADRUPOLE(" QD2 ",0.187D0, K1= -2.3368D0);  
  QFA1= QUADRUPOLE(" QFA1",0.448D0, K1= 2.8856D0);  
  QFA2= QUADRUPOLE(" QFA2",0.448D0, K1= 2.8856D0);  

!!! 1/2 mad-x value
ksf=(-41.67478927130080d0+0.3392376315938252d0);ksd= (56.36083889436033d0-0.1043679358857811d0);
   sf=sextupole ("sf",2.d0*0.1015d0, K2= ksf);
   sd= sextupole("sd", 2.d0*0.1015d0, K2= ksd);

 VC5=marker("vc5");
ALPHA=0.17453292519943295769236907684886d0;
 
LBEND=0.86621d0;
 
 
BEND = RBEND("BEND", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
BEND1 = RBEND("BEND1", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
 
CAV=RFCAVITY("CAV",L=0.0000d0,VOLT=-1.0d0,REV_FREQ=500.0d6)


sfline=1*sf;
sdline=1*sd;

SUP1=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

SUPb=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND1+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

ALS = 11*sup1+supb+cav;
 
ALS = .ring.ALS

call survey(ALS)


if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(ALS,SIG,cut)
endif
end subroutine build_ALS
